//
// Created by nsy on 2020/11/20.
//

#include <pcl/conversions.h>
#include "../../include/sensor_info/camera_info.h"


CameraInfo::CameraInfo(CameraInfo::CameraResolution resolution) {
    useDefaultParams(resolution);
}

/**
 * 使用默认参数
 * @return
 */
void CameraInfo::useDefaultParams(CameraInfo::CameraResolution resolution) {

    switch (resolution) {
        case HD720:
            focal_x_ = focal_y_ = 525.397;
            center_x_ = 610.086;
            center_y_ = 365.801;
            break;
        case HD2K:
            focal_x_ = focal_y_ = 1059.12;
            center_x_ = 1064;
            center_y_ = 635.01;
            break;
    }

    pose_ << 0.005339522383540407, -0.9996093334485746, -0.027434831568868, -0.3248629668208393 + 0.02,
            -0.02328667005922458, 0.02730348814697614, -0.9993559178453695, -0.04994633578153714,
            0.9997145695138532, 0.005974949163333465, -0.02313178515002878, 0.01976535732176605,
            0, 0, 0, 1;
}

/**
 * 设置相机焦距
 * @param[input] focal
 * @return
 */
void CameraInfo::setFocal(double focal) {

    return setFocal(focal, focal);

}

/**
 * 设置相机焦距
 * @param[input] focal_x
 * @param[input] focal_y
 * @return
 */
void CameraInfo::setFocal(double focal_x, double focal_y) {
    // 焦距必须大于0
    if (focal_x <= 0 || focal_y <= 0) {
        PCL_ERROR("focal error");
    }

    focal_x_ = focal_x;
    focal_y_ = focal_y;
}

/** 获取水平焦距 */
double CameraInfo::getFocalX() const {
    return focal_x_;
}

/** 获取垂直焦距 */
double CameraInfo::getFocalY() const {
    return focal_y_;
}

/**
 * 设置位姿变换矩阵
 * @param[input] pose
 * @return
 */
void CameraInfo::setPose(Eigen::Matrix<double, 4, 4> pose) {

    pose_ = pose;
    
}

/** 获取位姿变换矩阵 */
Eigen::Matrix<double, 4, 4> CameraInfo::getPose() {
    return pose_;
}

Eigen::Matrix<double, 4, 4> CameraInfo::getTransformMatrix() {

    Eigen::Matrix<double, 4, 4> mat;
    mat << 1,0,0,0, 0,-1,0,0, 0,0,-1,0, 0,0,0,1;

    return mat * pose_;
}

/** 设置中心 */
void CameraInfo::setCenter(double center_x, double center_y) {

    center_x_ = center_x;
    center_y_ = center_y;
    

}

/** 获取水平中心 */
double CameraInfo::getCenterX() const {
    return center_x_;
}

/** 获取垂直中心 */
double CameraInfo::getCenterY() const {
    return center_y_;
}

/**
 * 获得内参矩阵
 *[focal_x     0     center_x]
 *[   0     focal_y  center_y]
 *[   0        0        1    ]
 * @return 相机内参矩阵
 */
Eigen::Matrix3d CameraInfo::getParamMatrix() const {

    Eigen::Matrix3d mat;
    mat << focal_x_, 0, center_x_,
            0, focal_y_, center_y_,
            0,    0,       1;
    return mat;
}

/** 设置对应照片路径 */
void CameraInfo::setImagePath(const std::string path) {

    // 读取图像
    cv::Mat image = cv::imread(path);
    // 测试读取图像是否成功
    if (image.empty()) {
        PCL_ERROR("图像读取失败");

    }
    image_path_ = path;
    width_ = image.cols;
    height_ = image.rows;

    

}

/** 获取对应照片路径 */
std::string CameraInfo::getImagePath() {
    return image_path_;
}

/** 获取照片宽度高度 */
int CameraInfo::getWidth() const {
    return width_;
}

int CameraInfo::getHeight() const {
    return height_;
}
